﻿using UnityEngine;
using System.Collections;

public class Rigid_Bunny : MonoBehaviour
{
    public float gravity = 9.81f;
    public Vector3 InitialSpeed = new Vector3(5f, 2f, 0);
    private float u_N = 0.6f;
    public float u_N_Basic = 0.6f;
    public float u_T = 0.5f;

    bool launched = false;
    float dt = 0.015f;
    Vector3 v = new Vector3(0, 0, 0);   // velocity
    Vector3 w = new Vector3(0, 0, 0);   // angular velocity

    float mass;                                 // mass
    Matrix4x4 I_ref;                            // reference inertia

    float linear_decay = 0.999f;                // for velocity decay
    float angular_decay = 0.98f;


    Mesh mesh;
    Vector3[] vertices;


    // Use this for initialization
    void Start()
    {
        mesh = GetComponent<MeshFilter>().mesh;
        vertices = mesh.vertices;

        float m = 1;
        mass = 0;
        for (int i = 0; i < vertices.Length; i++)
        {
            mass += m;
            float diag = m * vertices[i].sqrMagnitude;
            I_ref[0, 0] += diag;
            I_ref[1, 1] += diag;
            I_ref[2, 2] += diag;
            I_ref[0, 0] -= m * vertices[i][0] * vertices[i][0];
            I_ref[0, 1] -= m * vertices[i][0] * vertices[i][1];
            I_ref[0, 2] -= m * vertices[i][0] * vertices[i][2];
            I_ref[1, 0] -= m * vertices[i][1] * vertices[i][0];
            I_ref[1, 1] -= m * vertices[i][1] * vertices[i][1];
            I_ref[1, 2] -= m * vertices[i][1] * vertices[i][2];
            I_ref[2, 0] -= m * vertices[i][2] * vertices[i][0];
            I_ref[2, 1] -= m * vertices[i][2] * vertices[i][1];
            I_ref[2, 2] -= m * vertices[i][2] * vertices[i][2];
        }
        I_ref[3, 3] = 1;
    }

    Matrix4x4 Get_Cross_Matrix(Vector3 a)
    {
        //Get the cross product matrix of vector a
        /* 对a(x,y,z):
           [0,-z, y, 0,
            z, 0,-x, 0,
           -y, x, 0, 1,
            0, 0, 0, 1] */
        Matrix4x4 A = Matrix4x4.zero;
        A[0, 0] = 0;
        A[0, 1] = -a[2];
        A[0, 2] = a[1];
        A[1, 0] = a[2];
        A[1, 1] = 0;
        A[1, 2] = -a[0];
        A[2, 0] = -a[1];
        A[2, 1] = a[0];
        A[2, 2] = 0;
        A[3, 3] = 1;
        return A;
    }
    Matrix4x4 QuaternionToMatrix(Quaternion q)
    {
        //float q0 = q.w;
        //float q1 = q.x;
        //float q2 = q.y;
        //float q3 = q.z;
        //Matrix4x4 R = new Matrix4x4(
        //    new Vector4(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3, 2 * q1 * q2 - 2 * q0 * q3, 2 * q1 * q3 + 2 * q0 * q2, 0),
        //    new Vector4(2 * q1 * q2 + 2 * q0 * q3, q0 * q0 + q2 * q2 - q1 * q1 - q3 * q3, 2 * q2 * q3 - 2 * q0 * q1, 0),
        //    new Vector4(2 * q1 * q3 - 2 * q0 * q2, 2 * q2 * q3 + 2 * q0 * q1, q0 * q0 + q3 * q3 - q1 * q1 - q2 * q2, 0),
        //    new Vector4(0, 0, 0, 1));
        //return R;
        return Matrix4x4.Rotate(q);//才发现这个库函数
    }
    Matrix4x4 MatrixProductFloat(Matrix4x4 m, float f)
    {
        for (int i = 0; i <= 3; i++)
            for (int j = 0; j <= 3; j++)
                m[i, j] *= f;
        return m;
    }
    Vector4 MatrixProductVector4(Matrix4x4 m, Vector4 v)
    {
        Vector4 res = new Vector4(m.m00 * v[0] + m.m01 * v[1] + m.m02 * v[2] + m.m03 * v[3],
            m.m10 * v[0] + m.m11 * v[1] + m.m12 * v[2] + m.m13 * v[3],
            m.m20 * v[0] + m.m21 * v[1] + m.m22 * v[2] + m.m23 * v[3],
            m.m30 * v[0] + m.m31 * v[1] + m.m32 * v[2] + m.m33 * v[3]
            );
        return res;
    }
    Matrix4x4 MatrixAdd(Matrix4x4 a, Matrix4x4 b)
    {
        Matrix4x4 res = new Matrix4x4();
        for (int i = 0; i <= 3; i++)
            for (int j = 0; j <= 3; j++)
                res[i, j] = a[i, j] + b[i, j];
        return res;
    }
    Matrix4x4 MatrixSub(Matrix4x4 a, Matrix4x4 b)
    {
        Matrix4x4 res = new Matrix4x4();
        for (int i = 0; i <= 3; i++)
            for (int j = 0; j <= 3; j++)
                res[i, j] = a[i, j] - b[i, j];
        return res;
    }
    Vector3 GetTotalForce()
    {
        Vector3 F = new Vector3(0, -gravity, 0) * mass;
        return F;
    }

    // In this function, update v and w by the impulse due to the collision with
    //a plane <P, N>
    void Collision_Impulse(Vector3 P, Vector3 N)
    {
        N = N.normalized;
        Matrix4x4 R = Matrix4x4.Rotate(transform.rotation);

        int numOfCollision = 0;
        Vector3 avg_Collision_Point = Vector3.zero; //平均碰撞点
        Vector3 Rri = Vector3.zero;
        Vector3 v_i = Vector3.zero;
        for (int i = 0; i < vertices.Length; i++)
        {
            Rri = R.MultiplyVector(vertices[i]);
            Vector3 pos_i = transform.position + Rri;
            //未发生碰撞
            if (Vector3.Dot(pos_i - P, N.normalized) >= 0)
                continue;
            v_i = v + Vector3.Cross(w, Rri);
            //粒子速度已经朝外
            if (Vector3.Dot(v_i, N) >= 0)
                continue;
            avg_Collision_Point += vertices[i];
            numOfCollision++;
        }
        //模型未发生碰撞
        if (numOfCollision == 0)
            return;

        Vector3 avg_vertex = avg_Collision_Point /= numOfCollision;
        Rri = R.MultiplyVector(avg_vertex);
        v_i = v + Vector3.Cross(w, Rri);

        //解出vi_new
        Vector3 VN_i = Vector3.Dot(v_i, N) * N;
        Vector3 VT_i = v_i - VN_i;
        float a = Mathf.Max(1.0f - u_T * (1.0f + u_N) * VN_i.magnitude / VT_i.magnitude, 0);
        VN_i *= -u_N;
        VT_i *= a;
        Vector3 v_i_new = VN_i + VT_i;
        //解出冲量j
        Matrix4x4 I = R * I_ref * R.transpose;
        Matrix4x4 K = MatrixSub(MatrixProductFloat(Matrix4x4.identity, 1 / mass), Get_Cross_Matrix(Rri) * I.inverse * Get_Cross_Matrix(Rri));
        Vector3 J = K.inverse.MultiplyVector(v_i_new - v_i);
        //更新整体的v,w
        v += J / mass;
        w += I.inverse.MultiplyVector(Vector3.Cross(Rri, J));
    }

    // Update is called once per frame
    void Update()
    {

    }
    private void FixedUpdate()
    {
        //Game Control
        dt = Time.fixedDeltaTime;
        if (Input.GetKey("r"))
        {
            transform.position = new Vector3(0, 0.6f, 0);
            u_N = u_N_Basic;
            launched = false;
        }
        if (Input.GetKey("l"))
        {
            v = InitialSpeed;
            launched = true;
        }
        //为减少抖动，需对u_N做衰减处理
        if (Mathf.Abs(v.y) < 1.0f)
        {
            u_N = u_N_Basic * (Mathf.Abs(v.y) / 1.0f);
        }
        if (!launched) return;
        v *= linear_decay;
        w *= angular_decay;
        // Part I: Update velocities
        v += GetTotalForce() / mass * dt;

        // Part II: Collision Impulse
        Collision_Impulse(new Vector3(0, 0.01f, 0), new Vector3(0, 1, 0));
        Collision_Impulse(new Vector3(2, 0, 0), new Vector3(-1, 0, 0));

        // Part III: Update position & orientation

        //Update linear status
        Vector3 x = transform.position;
        x += v * dt;

        //Update angular status

        Quaternion q = transform.rotation;
        Vector3 wt = 0.5f * dt * w;
        Quaternion dq = new Quaternion(wt.x, wt.y, wt.z, 0.0f) * q;
        q.Set(q.x + dq.x, q.y + dq.y, q.z + dq.z, q.w + dq.w);


        // Part IV: Assign to the object
        transform.position = x;
        transform.rotation = q;
    }
}